#ifndef OMPL_GEOMETRIC_STATE_RECORDER_
#define OMPL_GEOMETRIC_STATE_RECORDER_

#include <vector>
#include <string>
#include <fstream>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/geometric/PathGeometric.h>

static void record_state(std::vector<ompl::base::State*>& refStates, const std::string& strFileName)
{
    std::fstream fOutput;
    fOutput.open( strFileName.c_str(), std::ios_base::out | std::ios_base::ate );
    for ( std::vector<ompl::base::State*>::iterator itLoop = refStates.begin(); refStates.end() != itLoop; ++itLoop )
    {
        ompl::base::SE2StateSpace::StateType* pState = (*itLoop)->as<ompl::base::SE2StateSpace::StateType>();

        if ( NULL == pState )
        {
            continue;
        }

        fOutput << pState->getX() <<"," << pState->getY() << std::endl;
    }

    fOutput.close();
}

static void record_state(const ompl::geometric::PathGeometric& path, const std::string& strFileName)
{
    std::fstream fOutput;
    fOutput.open( strFileName.c_str(), std::ios_base::out | std::ios_base::ate );

    std::size_t stateCount = path.getStateCount();

    for (int i = 0; i < stateCount; i++)
    {
        const ompl::base::SE2StateSpace::StateType * pState = path.getState(i)->as<ompl::base::SE2StateSpace::StateType>();

        if ( NULL == pState )
        {
            continue;
        }

        fOutput << pState->getX() <<"," << pState->getY() << std::endl;
    }

    fOutput.close();
}

#endif